package Behaviors;

import lejos.nxt.Sound;
import lejos.robotics.navigation.Pose;
import lejos.robotics.subsumption.Behavior;
import sharedData.DebugItem;
import sharedData.MapData;
import sharedData.SharedConstants;
import NXTExplorer.SensorParameter;

/**
 * Bump behavior from subsumption architecture. Handles situations when
 * TouchSensor is pressed.
 * 
 * @author Didzis Petkus, Danny Johansen, Liviu Berheci
 * 
 */
public class Bump implements Behavior {

	private static final double ROTATE_ANGLE = -180;
	private SensorParameter paramObj;
	private final int BUCKUP_DIST = -30;
	public Bump(SensorParameter params) {
		paramObj = params;
	}

	@Override
	public boolean takeControl() {
		return paramObj.getTouchSensor().isPressed();
	}

	@Override
	public void action() {

		sendDebug("Bump action");

		paramObj.getPilot().stop();

		Sound.buzz();

		sendData();

		paramObj.getPilot().travel(BUCKUP_DIST);
		//TODO: Maybe we want to do something else just than change direction??
		paramObj.getPilot().rotate(ROTATE_ANGLE);
	}

	@Override
	public void suppress() {
	}

	/**
	 * Adds the robot position and sensor readings to the data queue based on
	 * given pose.
	 * 
	 * @param pose
	 *            - The given Pose object
	 */
	public void sendData() {
		Pose p = paramObj.getPoseProvider().getPose();
		MapData data = new MapData(SharedConstants.DATA_TYPE_BUMP_DATA,
				p.getX(), p.getY(), p.getHeading(), paramObj.getLeftUssr()
						.getRange(), paramObj.getRightUssr().getRange(),
				paramObj.getCenterUssr().getRange());
		paramObj.getQueue().add(data);
	}

	/**
	 * Adds the debug information to the data queue with the given message.
	 * 
	 * @param msg
	 *            - the given message
	 */
	public void sendDebug(String msg) {
		paramObj.getQueue().add(
				new DebugItem(SharedConstants.DATA_TYPE_DEBUG, paramObj
						.getPoseProvider().getPose().getX(), paramObj
						.getPoseProvider().getPose().getY(), paramObj
						.getPoseProvider().getPose().getHeading(), msg));
	}

}
